#include <Servo.h>
int leftCounter=0,  rightCounter=0;
int t=1;
int k1=1;
int k2=1;
unsigned long time = 0, old_time = 0; // 时间标记
unsigned long time1 = 0; // 时间标记
float lv,rv;//左、右轮速度
Servo Base;

#define STOP        0//停止
#define FORWARD     1//前进
#define BACKWARD    2//后退
#define TURNLEFT    3//左转
#define TURNRIGHT   4//右转
#define CHANGESPEED 10//改变速度
#define OUTO        6//切换循迹
#define kai         7//机械爪打开
#define guan        8//机械爪关闭
#define song        9//机械爪慢慢松开
#define jing        5//机械爪慢慢变紧
int pos;
int led1 = A0;
int led2 = A1;
int leftMotor1 = A2;//左1
int leftMotor2 = A3;//
int rightMotor1 = A4;//
int rightMotor2 = A5;
bool speedLevel=0;
int trac1 = 10; //从车头方向的最右边开始排序
int trac2 = 11; 
int trac3 = 12; 
int trac4 = 13; 
int leftPWM = 5;
int rightPWM = 6;
int cm;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Base.attach(9);                                                                                                                                              
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
  pinMode(leftPWM, OUTPUT);
  pinMode(rightPWM, OUTPUT);
  
  //寻迹模块D0引脚初始化
  pinMode(trac1, INPUT);
  pinMode(trac2, INPUT);
  pinMode(trac3, INPUT);
  pinMode(trac4, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
    
 
 
  if(Serial.available()>0)
  {
    s:char cmd = Serial.read();
    
    Serial.print(cmd);

    motorRuna(cmd);
    if(speedLevel)  //根据不通的档位输出不同速度
    {
      analogWrite(leftPWM, 120);
      analogWrite(rightPWM, 120);
    }
    else
    {
      analogWrite(leftPWM, 255);
      analogWrite(rightPWM, 255);
    }

    while(cm==1)
     {
      tracing();
      if(Serial.available()>0)
      {
        t=1;

        goto s;
      }
     }
    while(cm==2&&k1==1)
     { 
   
      if(Serial.available()>0)
      {
        goto s;
      }
     }
  }  
   
}


void motorRuna(int cmd)
{
  switch(cmd){
     case kai:
    {
      pos=120;
      Base.write(pos);
     delay(15);break;
    }
    case guan:{
      pos=0;
      Base.write(pos);
      delay(15);break;
    }
    case song:{
    pos=pos+3;Base.write(pos);
      delay(15);break;}
    case jing:{
      pos=pos-3;Base.write(pos);
      delay(15);break;}
    
    case FORWARD:
      Serial.println("FORWARD"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      cm=0;
     // digitalWrite(led1, LOW);
     // digitalWrite(led2, LOW);
      break;
     case BACKWARD:
      Serial.println("BACKWARD"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      cm=0;
     // digitalWrite(led1, LOW);
     // digitalWrite(led2, LOW);
      break;
     case TURNLEFT:
      Serial.println("TURN  LEFT"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      cm=0;
     // digitalWrite(led1, LOW);
      //digitalWrite(led2, LOW);
      break;
     case TURNRIGHT:
      Serial.println("TURN  RIGHT"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      cm=0;
      //digitalWrite(led1, LOW);
      //digitalWrite(led2, LOW);
      break;
     case OUTO:
     cm=1;//循迹 
     break;
    
     case CHANGESPEED:
     cm=0;
      Serial.println("CHANGE SPEED"); //输出状态
      if(speedLevel)  //接收到换挡命令的时候切换档位
        speedLevel=0;
      else
        speedLevel=1;
      break;
     default:
      Serial.println("STOP"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
      cm=0;
      //digitalWrite(led1, HIGH);
     // digitalWrite(led2, HIGH);
  }
}

void motorRun(int cmd,int value)
{
  analogWrite(leftPWM, value);  //设置PWM输出，即设置速度
  analogWrite(rightPWM, value);
  switch(cmd){
    case FORWARD:
      Serial.println("FORWARD"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
           // digitalWrite(led1, LOW);
      //digitalWrite(led2, LOW);
      break;
     case BACKWARD:
      Serial.println("BACKWARD"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
           // digitalWrite(led1, LOW);
     // digitalWrite(led2, LOW);
      break;
     case TURNLEFT:
      Serial.println("TURN  LEFT"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
           // digitalWrite(led1, LOW);
     // digitalWrite(led2, LOW);
      break;
     case TURNRIGHT:
      Serial.println("TURN  RIGHT"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
           // digitalWrite(led1, LOW);
     // digitalWrite(led2, LOW);
      break;
     default:
      Serial.println("STOP"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
           // digitalWrite(led1, HIGH);
     // digitalWrite(led2, HIGH);
  }
}

void tracing()
{
  int data[4];
  data[0] = digitalRead(10);
  data[1] = digitalRead(11);
  data[2] = digitalRead(12);
  data[3] = digitalRead(13);
//红外判断代码
  if((data[1]||data[2])&&!data[3]&&!data[0]){
    motorRun(FORWARD,250 );
  }
 
  if(data[0])  //右边检测到黑线
  {
    motorRun(TURNRIGHT, 90);
  }
   
  if(data[3])  //左边检测到黑线
  {
    motorRun(TURNLEFT  ,90);
  }


  if(t==0)
  {
    motorRun(STOP, 0);

     /* digitalWrite(led1, HIGH);
      digitalWrite(led2, HIGH);*/
  }
  
  Serial.print(data[0]);
  Serial.print("---");
  Serial.print(data[1]);
  Serial.print("---");
  Serial.print(data[2]);
  Serial.print("---");
  Serial.println(data[3]);
}